package roborl;


import java.awt.Graphics2D;
import java.awt.Shape;
import java.util.Vector;

import net.phys2d.math.Vector2f;
import net.phys2d.raw.ActuatorJoint;
import net.phys2d.raw.BasicJoint;
import net.phys2d.raw.Body;
import net.phys2d.raw.DistanceJoint;
import net.phys2d.raw.Joint;
import net.phys2d.raw.StaticBody;
import net.phys2d.raw.World;
import net.phys2d.raw.shapes.Box;
import net.phys2d.raw.shapes.Circle;
import net.phys2d.raw.test.AbstractDemo;

public class PhysicsWorld extends AbstractDemo {
	Vector<PhysRobot> roboVector;

	public PhysicsWorld() {
		super("Physics World");
		roboVector = new Vector<PhysRobot>();
	}

	public void addRobot(PhysRobot robo) {
		roboVector.add(robo);
		Vector<Object> parts = robo.getParts();

		for (Object obj : parts) {
			if (obj instanceof Joint) {
				Joint joint = (Joint) obj;
				world.add(joint);
			} else if (obj instanceof Body) {
				Body body = (Body) obj;
				
//				for (Object obj1 : parts) {
//					if (obj1 instanceof Body) {
//						((Body) obj1).addExcludedBody(body);
//					}
//				}
				body.setBitmask(1);
				world.add(body);
			}
		}
	}

	public synchronized void removeRobot(PhysRobot robo) {
		roboVector.remove(robo);
		Vector<Object> parts = robo.getParts();

		for (Object obj : parts) {
			if (obj instanceof Joint) {
				Joint joint = (Joint) obj;
				world.remove(joint);
			} else if (obj instanceof Body) {
				Body body = (Body) obj;
				world.remove(body);
			}
		}
	}

	protected void update() {
		for (PhysRobot physRobot : roboVector) {
			physRobot.update();
		}
	}

	@Override
	protected void init(World world) {
		Body body11 = new StaticBody("Ground1", new Box(1000.0f, 20.0f));
		body11.setPosition(500.0f, 400);
		world.add(body11);
		body11.setRestitution(0);
		body11.setFriction(2);
	}

	@Override
	protected void renderGUI(Graphics2D g) {
		super.renderGUI(g);
	}

	public static void main(String s[]) {
		PhysicsWorld physicsWorld = new PhysicsWorld();
		physicsWorld.start();
	}
}
